{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "c843da58",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import cv2 \n",
    "import pandas as pd\n",
    "import yaml\n",
    "\n",
    "# 设置Numpy的打印选项\n",
    "# 精确位数3，不启用科学计数法\n",
    "np.set_printoptions(precision=3, suppress=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "4979dc4f",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 载入九点标定法的配置文件\n",
    "handeye_9points_config = None\n",
    "with open(\"config/handeye_calibration/handeye_9points.yaml\", 'r', encoding='utf-8') as f:\n",
    "\thandeye_9points_config = yaml.load(f.read(), Loader=yaml.SafeLoader)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "9d60e413",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 新的相机内参矩阵(畸变去除后)\n",
    "M_intrisic_new = np.loadtxt('config/usb_camera/M_intrisic_new.txt', delimiter=',')\n",
    "# 因为是按畸变去除后的图像来计算，因此畸变系数为零向量\n",
    "distor_coeff = np.float64([0, 0, 0, 0, 0]).astype(\"float32\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "415fd7b3",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "9点在像素坐标系的坐标: \n",
      "[[ 225.  147.]\n",
      " [ 667.  141.]\n",
      " [1099.  129.]\n",
      " [ 180.  401.]\n",
      " [ 678.  396.]\n",
      " [1165.  377.]\n",
      " [ 121.  716.]\n",
      " [ 690.  705.]\n",
      " [1248.  680.]]\n"
     ]
    }
   ],
   "source": [
    "# 载入九点在2D图像中的坐标\n",
    "img_9points = np.loadtxt(\"config/handeye_calibration/img9points.txt\", delimiter=',')\n",
    "print(f\"9点在像素坐标系的坐标: \\n{img_9points}\")\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "c93ac1bb",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "9点在工作台坐标系的坐标: \n",
      "[[ 45.  65.   0.]\n",
      " [ 45.   0.   0.]\n",
      " [ 45. -65.   0.]\n",
      " [  0.  65.   0.]\n",
      " [  0.   0.   0.]\n",
      " [  0. -65.   0.]\n",
      " [-45.  65.   0.]\n",
      " [-45.   0.   0.]\n",
      " [-45. -65.   0.]]\n"
     ]
    }
   ],
   "source": [
    "# 9点在工作台坐标系的坐标\n",
    "# ws_9points_df = pd.read_excel(\"config/九点坐标-工作台坐标系.xlsx\")\n",
    "# ws_9points = np.float64(ws_9points_df.to_numpy())[:, 1:]\n",
    "w = handeye_9points_config[\"board_width\"]\n",
    "h = handeye_9points_config[\"board_height\"]\n",
    "# P0点的坐标\n",
    "x0 = 0.5 * h\n",
    "y0 = 0.5 * w\n",
    "ws_9points = np.float64([\n",
    "\t[x0, y0, 0], \t#P0\n",
    "\t[x0, 0, 0], \t#P1\n",
    "\t[x0, -y0, 0], \t#P2\n",
    " \t[0, y0, 0], \t#P3\n",
    "\t[0, 0, 0], \t\t#P4\n",
    "\t[0, -y0, 0], \t#P5\n",
    "\t[-x0, y0, 0], \t#P6\n",
    "\t[-x0, 0, 0], \t#P7\n",
    "\t[-x0, -y0, 0], \t#P8\n",
    "])\n",
    "print(f\"9点在工作台坐标系的坐标: \\n{ws_9points}\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "66cbe2e3",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 用solvePnPRansac会更稳一些\n",
    "ret, rvec, tvec, *_ = cv2.solvePnPRansac(ws_9points, img_9points, \\\n",
    "                    M_intrisic_new, distor_coeff,  flags=cv2.SOLVEPNP_ITERATIVE)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "b2c2d425",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "T_cam2ws\n",
      "[[ -0.036  -0.999  -0.013  -2.619]\n",
      " [ -0.839   0.024   0.544 -18.207]\n",
      " [ -0.544   0.03   -0.839 189.405]\n",
      " [  0.      0.      0.      1.   ]]\n"
     ]
    }
   ],
   "source": [
    "# 构造相机坐标系到工作台坐标系的变换矩阵\n",
    "T_cam2ws = np.eye(4)\n",
    "T_cam2ws[:3, :3] = cv2.Rodrigues(rvec)[0]\n",
    "T_cam2ws[:3, 3] = tvec.reshape(-1)\n",
    "print(\"T_cam2ws\")\n",
    "print(T_cam2ws)\n",
    "np.savetxt(\"config/handeye_calibration/T_cam2ws.txt\", T_cam2ws, fmt='%.3f', delimiter=\",\")"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
